package bojan.robots.motion;

import sim.util.Double2D;

public class Motion {
  double step;

public Motion(double step) {
	super();
	this.step = step;
}

public Double2D nextPosition(Double2D origin, Double2D goal) {
	Double2D a = step(origin, goal);
	return origin.add(a);
}

public Double2D step(Double2D origin, Double2D goal) {
	Double2D a = goal.subtract(origin);
	if (a.length() > step) {
		a = a.resize(step);
	}
	return a;
}



//public Double2D alternativePosition(Double2D loc, Double2D goal) {
//	// TODO Auto-generated method stub
//	Double2D a = move(loc, goal);
//	Double2D x = loc.add(new Double2D(a.x, 0));
//	Double2D y = loc.add(new Double2D(0, a.y));
//	if (y.distance(loc) < x.distance(loc) ) {
//		return y;
//	}
//	return x;
//}


}
